#include "string.h"		
#include "stdio.h"
#include "delay.h"
#include "sys.h"
#include "oled.h"   //64X128 
#include "EXTI.h"
#include "bmp.h"
#include "mpu6050.h"
#include "inv_mpu.h"
#include "inv_mpu_dmp_motion_driver.h" 
#include "stm32f10x_tim.h"
#include "stm32f10x_exti.h"
#include "math.h"
#include "USART.h"

unsigned char temp[50];	
int count;
int num = 0;
int MPU;
int cms;
float pitch,roll,yaw; 		
short aacx,aacy,aacz;	
short gyrox,gyroy,gyroz;	



int main(void)
{
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
	
	delay_init();
	uart_init(115200);
	OLED_Init();

	OLED_ShowString(0,0,"oled ok",8,1);
	printf("oled ok");
	sprintf(temp,"MPU_DEVICE_ID:0x%x  ",MPU_DEVICE_ID_REG);
	OLED_ShowString(0,1,(unsigned char *)temp,8,1);	
	sprintf(temp,"MPU_IIC_AD0:0x%x  ",MPU_ADDR);
	OLED_ShowString(0,2,(unsigned char *)temp,8,1);	

	MPU = MPU_Init();
	printf("MPU_Init");
	OLED_ShowString(0,4,"MPU_Init:",8,1);	
	OLED_ShowNum(54,4,MPU,1,8,1);	

	MPU = mpu_dmp_init();
	printf("mpu_dmp_init");
	OLED_ShowString(0,5,"mpu_dmp_init:",8,1);	
	OLED_ShowNum(78,5,MPU,1,8,1);

	OLED_Refresh();
	
	while(MPU_Init())
	{

		OLED_ShowString(0,4,(unsigned char *)"MPU_Init error",8,1);	
		OLED_Refresh();
	}

	while(mpu_dmp_init())
	{
		OLED_ShowString(0,3,(unsigned char *)"MPU6050 error",8,1);
		OLED_Refresh();
		delay_ms(200);
	}

	TIM2_Init(10000,7200);

	OLED_Clear();

	while(1)
	{
		if(mpu_dmp_get_data(&pitch,&roll,&yaw)==0)
		{ 
			unsigned char data[50];
			cms = 0;
			cms++;

			MPU_Get_Accelerometer(&aacx,&aacy,&aacz);
			MPU_Get_Gyroscope(&gyrox,&gyroy,&gyroz);

			magic_cube();
			count++;
			printf("1:%0.2f\r\n",pitch);
			printf("2:%0.2f\r\n",roll);
			printf("3:%0.2f\r\n",yaw);
			sprintf(data,"pitch:%0.2f",pitch);
			OLED_ShowString(0,1,(unsigned char *)data,8,1);
			sprintf(data,"roll:%0.2f",roll);
			OLED_ShowString(0,2,(unsigned char *)data,8,1);
			sprintf(data,"yaw:%0.2f",yaw);
			OLED_ShowString(0,3,(unsigned char *)data,8,1);
			OLED_Re(1,0,4,80);

		}
	}
}

void TIM2_IRQHandler()
{
	if(TIM_GetITStatus(TIM2,TIM_IT_Update)==SET){

	TIM_ClearITPendingBit(TIM2,TIM_IT_Update);

	sprintf(temp,"MPU:%d  ",count);
	OLED_ShowString(88,7,(unsigned char *)temp,8,1);

	sprintf(temp,"FPS:%d  ",cms);
	OLED_ShowString(88,0,(unsigned char *)temp,8,1);		

	sprintf(temp,"count:%d     ",num++);
	OLED_ShowString(0,7,(unsigned char *)temp,8,1);
	count = 0 ;
	OLED_Refresh();
	}
}


